#ifndef __DALELIB_AGENTBONE_H__
#define __DALELIB_AGENTBONE_H__

#include <dalelib/graphics.h>
#include <dalelib/maths/v3.h>
#include <dalelib/maths/m4.h>

using namespace dalelib;


class AgentBone
{
private:

	friend class Agent;

	AgentBone* _parent;
//	v3 _local_pos;
//	v3 _global_pos;
	m4 _local_mat;
	m4 _global_mat;
	m4 _global_mat_inv;
	m4 _global_mat_rest_inv;
	m4 _rest_to_pose_mat;
//	v3 _pos;
	v3 _ang;
	v3 _offset;
	v3 _col;
	float _scale;
	int _id;

	float _length;
	float _length_modifier;

	string _name;
	string _parent_name;
	int _parent_index;

	int _channels[6];


	float _len;
	v3 _rest_pos;
	m4 _rest_rot;

public:

	AgentBone(AgentBone* _Parent, const v3 _Start=v3(0,0,0), const v3 _End=v3(0,1,0), const v3 _Col=v3(1,1,1), const float _Scale=1.f)
		: _parent( _Parent )
		, _col( _Col )
		, _scale( _Scale )
		, _length_modifier( 1.f )
	{

		v3 pointing = _Start-_End;
		_length = pointing.normalise();
		if( _length<0.001f ) { pointing = v3(0,0,1); _length = 1.f; }
		v3 up(0,1,0);
		if( fabs(v3::dot(up,pointing))>0.99f ) up = v3(0,0,1);

		_global_mat.lookAt( _Start, _End, up );

		if( _parent )
			_local_mat = _parent->getGlobalMatInv()*_global_mat;
		else
			_local_mat = _global_mat;

		calculateTransforms();

		for(int i=0; i<6; ++i) _channels[i] = -1;

	}

	~AgentBone() { }

	float& getLengthModifier() { return _length_modifier; }
	int &getID() { return _id; }
	int getChannel(const int _Index) { return _channels[_Index]; }
	void setChannel(const int _Index,const int _Value) { _channels[_Index] = _Value; }
	void setName(const string _Name) { _name = _Name; }
	string getName() { return _name; }
	AgentBone*& getParent() { return _parent; }
//	v3& getLocalPos() { return _local_pos; }
	v3 getGlobalX() { 		return v3( _global_mat[ 0], _global_mat[ 1], _global_mat[ 2] ); }
	v3 getGlobalY() { 		return v3( _global_mat[ 4], _global_mat[ 5], _global_mat[ 6] ); }
	v3 getGlobalZ() { 		return v3( _global_mat[ 8], _global_mat[ 9], _global_mat[10] ); }
	v3 getGlobalPos() { 	return v3( _global_mat[12], _global_mat[13], _global_mat[14] ); }
//	v3& getPos() { return _pos; }
	v3& getAng() { return _ang; }
	v3& getOffset() { return _offset; }
	m4& getLocalMat() { return _local_mat; }
	void setLocalPos(const v3 _Pos) { _global_mat[12]=_Pos.x(); _global_mat[13]=_Pos.y(); _global_mat[14]=_Pos.z(); }
	m4 getGlobalMat() const { return _global_mat; }
	m4 getGlobalMatInv() const { return _global_mat_inv; }
	m4 getGlobalMatRestInv() const { return _global_mat_rest_inv; }
	void setGlobalMatRestInv() { _global_mat_rest_inv = _global_mat_inv; }
	m4& getRestToPoseMat() { return _rest_to_pose_mat; }

	v3 getGlobalStartPos()
	{
		if(_parent)
			return _parent->getGlobalMat().transformPoint(v3(0,0,0));
		else
			return _global_mat.transformPoint(v3(0,0,0));
	}
	v3 getGlobalEndPos()
	{
		if(_parent)
			return _parent->getGlobalMat().transformPoint(_offset*_length_modifier);
		else
			return _global_mat.transformPoint(-_offset*_length_modifier);
	}

	float& getLength() { return _length; }

	void calculateTransforms()
	{
		if( _parent )
		{
			_global_mat = _parent->getGlobalMat()*_local_mat;
		}
		else
		{
			_global_mat = _local_mat;
		}
//		_global_mat_inv = _global_mat.fastInverse();
		_global_mat_inv = _global_mat.inverse();

		_rest_to_pose_mat = _global_mat*_global_mat_rest_inv;
	}

	void draw(const bool _DoubleLength=false)
	{
		if(_parent)
		{
			v3 off = _offset;
			if(_DoubleLength) off *= 2;
			glPushMatrix();
			glMultMatrixf( (float*)_parent->getGlobalMat().ptr() );
			glBegin(GL_LINES);
				glVertex3f(0,0,0);
				glVertex3f(off.x(),off.y(),off.z());
			glEnd();
			glPopMatrix();
		}
	}

	void dump()
	{
//		_pos.print("pos: ");
		_ang.print("ang: ");
		printf("channels"); for(int i=0; i<6; ++i) printf(" %d",_channels[i]); printf("\n");
	}

};

#endif
